#include "AVX03Data.h"

#include <stdlib.h>
#include <string>
#include <sstream>
#include <iomanip>

//Principal axes data
#define DEFAULT_AXIS_MIN -180.
#define DEFAULT_AXIS_MAX 180.
#define ROLL_MIN DEFAULT_AXIS_MIN
#define ROLL_MAX DEFAULT_AXIS_MAX
#define DEFAULT_ROLL (ROLL_MAX + ROLL_MIN) / 2.
#define PITCH_MIN DEFAULT_AXIS_MIN
#define PITCH_MAX DEFAULT_AXIS_MAX
#define DEFAULT_PITCH (PITCH_MAX + PITCH_MIN) / 2.
#define YAW_MIN DEFAULT_AXIS_MIN
#define YAW_MAX DEFAULT_AXIS_MAX
#define DEFAULT_YAW (YAW_MAX + YAW_MIN) / 2.

#define PRINCIPAL_AXES_DECIMALS 2
#define ROLL_DECIMALS PRINCIPAL_AXES_DECIMALS
#define PITCH_DECIMALS PRINCIPAL_AXES_DECIMALS
#define YAW_DECIMALS PRINCIPAL_AXES_DECIMALS

/*
 * Constructor
 */
AVX03Data::AVX03Data()
    : AbstractData("AVX03"),
    m_yaw(DEFAULT_YAW), m_pitch(DEFAULT_PITCH), m_roll(DEFAULT_ROLL),
    m_reserve1(0), m_reserve2(0), m_reserve3(0), m_reserve4(0)
{}
/*
 * Sets
 */
int AVX03Data::SetYaw(const float& value)
{
    if (value > YAW_MAX)
    {
        m_yaw = YAW_MAX;
        return 666;
    }
    else if (value < YAW_MIN)
    {
        m_yaw = YAW_MIN;
        return -666;
    }
 
    m_yaw = value;
    return 0;
}

int AVX03Data::SetPitch(const float& value)
{
    if (value > PITCH_MAX)
    {
        m_pitch = PITCH_MAX;
        return 666;
    }
    else if (value < PITCH_MIN)
    {
        m_pitch = PITCH_MIN;
        return -666;
    }

    m_pitch = value;
    return 0;
}

int AVX03Data::SetRoll(const float& value)
{
    if (value > ROLL_MAX)
    {
        m_roll = ROLL_MAX;
        return 666;
    }
    else if (value < ROLL_MIN)
    {
        m_roll = ROLL_MIN;
        return -666;
    }

    m_roll = value;
    return 0;
}

int AVX03Data::SetReserve1(const float& value)
{
    m_reserve1 = value;
    return 0;
}

int AVX03Data::SetReserve2(const float& value)
{    
    m_reserve2 = value;
    return 0;
}

int AVX03Data::SetReserve3(const float& value)
{
    m_reserve3 = value;
    return 0;
}

int AVX03Data::SetReserve4(const float& value)
{
    m_reserve4 = value;
    return 0;
}

/*
 * Data handling
 *
 * While calculating precision one should consider that:
 *  sign, and decimal dot, take away sentence space
 */
int AVX03Data::SerializeSentence(std::stringstream& stream, const bool& isSending)
{
     if (isSending)
     {
        // Send data to stream
        stream << m_numGeneratedSentences%100000000 << ','; // Ensure 8 bytes
        
        stream << std::setiosflags(std::ios::fixed);

        stream << std::setprecision(YAW_DECIMALS) << m_yaw << ',';        
        stream << std::setprecision(PITCH_DECIMALS) << m_pitch << ','; 
        stream << std::setprecision(ROLL_DECIMALS) << m_roll << ',';

        stream << std::setprecision(5); 
        stream << m_reserve1 << ',';
        stream << m_reserve2 << ',';
        stream << m_reserve3 << ',';
        stream << m_reserve4;
     }
     else
     {
        char comma;
        unsigned long numGeneratedSentences;
        float yaw, pitch, roll;
        float reserve1, reserve2, reserve3, reserve4;
        
        // Recieve data from stream
        stream >> numGeneratedSentences >> comma;
        stream >> yaw >> comma;
        stream >> pitch >> comma; 
        stream >> roll >> comma;
        stream >> reserve1 >> comma;
        stream >> reserve2 >> comma;
        stream >> reserve3 >> comma;
        stream >> reserve4;

        // Set the values
        SetYaw(yaw);       
        SetPitch(pitch);
        SetRoll(roll);
        SetReserve1(reserve1);
        SetReserve2(reserve2);
        SetReserve3(reserve3);
        SetReserve4(reserve4);
     }

     return 0;
}

/*
AVX03 Sentence format

1        10        20        30        40        50        60        70          82
|        |         |         |         |         |         |         |         | |
$AVX03,99999999,-180.00,-180.00,-180.00,0.000000,0.000000,0.000000,0.000000*6A
 |     |        |       |       |       |        |        |        |        |  
 |     |        |       |       |       |        |        |        |        Checksum data  
 |     |        |       |       |       |        |        |        |
 |     |        |       |       |       |        |        |        Floating Point Reserved Value 4 [8 bytes]  
 |     |        |       |       |       |        |        |
 |     |        |       |       |       |        |        Floating Point Reserved Value 3 [8 bytes]
 |     |        |       |       |       |        |
 |     |        |       |       |       |        Floating Point Reserved Value 2 [8 bytes]   
 |     |        |       |       |       | 
 |     |        |       |       |       Floating Point Reserved Value 1 [8 bytes]
 |     |        |       |       |
 |     |        |       |       Roll [6 bytes]  
 |     |        |       |
 |     |        |       Pitch [6 bytes]
 |     |        |
 |     |        Yaw [6 bytes]
 |     |
 |     Sentence ID (ordinal number) [up to 8 bytes]
 |
 Ducktape Works Sentence sent from AV X3 to for PC [5 bytes]
*/
